/**
 * @FilePath: \ros2\src\agv_perception\two_dimensional_code\qrodom\include\qrodom\qrodom.hpp
 * @Date: 2025-03-22 14:30:10
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-22 14:30:33
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#ifndef QRODOM_HPP
#define QRODOM_HPP

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <transform/transform.h>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/agv_event.hpp>
#include <agv_msgs/msg/init_pose.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <agv_srvs/srv/update_qr.hpp>
#include <tinyxml2.h>
#include <yaml-cpp/yaml.h>
#include <mutex>
#include <regex>

#define DEBUG_OUT(...) RCLCPP_INFO(rclcpp::get_logger("Qrodom"), __VA_ARGS__)
#define DEBUG_STREAM_OUT(info) \
    RCLCPP_INFO_STREAM(rclcpp::get_logger("Qrodom"), info)
#define DEBUG_OUT_THROTTLE(clock,s,info) \
    RCLCPP_INFO_STREAM_THROTTLE(rclcpp::get_logger("Qrodom"), clock, s, info) 
#define DEBUG_WARN_OUT(info) \
    RCLCPP_WARN_STREAM(rclcpp::get_logger("Qrodom"), info)
#define DEBUG_ERROR_OUT(info) \
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("Qrodom"), info)
#define DEBUG_FATAL_OUT(info) \
    RCLCPP_FATAL_STREAM(rclcpp::get_logger("Qrodom"), info)

#define DEBUG_WARN_ON_TRANSITION(condition, message) \
    do { \
        static bool last_##condition = true; \
        static bool triggered_##condition = false; \
        if (!(condition)) { \
            if (last_##condition && !triggered_##condition) { \
                DEBUG_WARN_OUT(message); \
                triggered_##condition = true; \
            } \
            last_##condition = false; \
        } else { \
            last_##condition = true; \
            triggered_##condition = false; \
        } \
    } while (0)

using namespace transform;

class Qrodom : public rclcpp::Node {
public:
    explicit Qrodom();
    ~Qrodom() override;

private:
    struct QrMapPoint {
	    float x = 0.0f;
	    float y = 0.0f;
	    float angle = 0.0f;
	    bool is_switch_point = false;

	    QrMapPoint() = default; 
	    QrMapPoint(float xpose, float ypose, float a, bool switchPoint)
	        : x(xpose), y(ypose), angle(a), is_switch_point(switchPoint) {}
	};

	struct MapPoint {
	    int qr_code_id = 0;
	    geometry_msgs::msg::Pose pose;
	    bool is_switch_point = false;

	    MapPoint() = default; 
	    MapPoint(int id, const geometry_msgs::msg::Pose& p, bool switchPoint)
	        : qr_code_id(id), pose(p), is_switch_point(switchPoint) {}
	};

    struct FloorInfo {
        std::map<uint32_t, QrMapPoint> qr_map;
        std::map<uint32_t, MapPoint> map_points;
    };

    struct KalmanFilter {
        double theta_pred;
        double p_pred;
        double k;
        double p;
        double theta_estimate;
        double a;    // 状态转移矩阵
        double q;    // 过程激励噪声协方差
        double r;    // 测量噪声协方差
      
        KalmanFilter(double a_init = 1.0, double q_init = 0.01, double r_init = 0.05)
            : theta_pred(0), p_pred(0), k(1), p(1), theta_estimate(0), a(a_init), q(q_init), r(r_init) {}

        double update(double theta_measurement) {
            // double thetaEstimate = thetaMeasurements[0];  // 初始角度估计值
            // 时间更新（预测）
            theta_pred = a * theta_estimate;
            p_pred = a * p * a + q;
            // 测量更新（校正）
            k = p_pred / (p_pred + r);
            theta_estimate = theta_pred + k * (theta_measurement - theta_pred);
            p = (1 - k) * p_pred;
            return theta_estimate;
        }
    };
    
    bool init_parameters();
    void initialize_components();
    void get_qrodom_paramters();
    void setup_communications();
    void start_timers();
    void qrmap_init();
    void init_pose();
    void process_qr_data();
    void handle_odometry(const agv_msgs::msg::OdometryLite::SharedPtr msg);
    void handle_qr_link(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
    void handle_pose(const agv_msgs::msg::PoseWithConfidence::SharedPtr msg);
    void handle_update_request(
        const std::shared_ptr<agv_srvs::srv::UpdateQr::Request> request,
        std::shared_ptr<agv_srvs::srv::UpdateQr::Response> response);
    void handle_move_info(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg);
    void handle_mode_info(const agv_msgs::msg::AGVMode::SharedPtr msg);
    void handle_floor_message(const agv_msgs::msg::InitPose::SharedPtr msg);

    void record_pose();
    void init_map();
    void publish_error();
    void calculate_position();
    double get_current_yaw();
    void set_current_yaw(double &yaw);
    agv_msgs::msg::OdometryLite get_current_odometry();
    void set_current_odometry(const agv_msgs::msg::OdometryLite &msg);
   
    double normal_angle(double angle);
    void update_odom_transform();
    void publish_tf_transform();

    inline double calculate_distance(const geometry_msgs::msg::Pose& a, const geometry_msgs::msg::Pose& b) {
      // ROS_INFO("ax:%.4f, ay:%.4f, bx:%.4f, by:%.4f", 
      // a.position.x, a.position.y, b.position.x, b.position.y);
      return sqrt(pow((a.position.x - b.position.x), 2) + 
        pow((a.position.y - b.position.y), 2));
    }

    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    rclcpp::Service<agv_srvs::srv::UpdateQr>::SharedPtr update_service_;
    rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odom_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr qr_link_sub_;
    rclcpp::Subscription<agv_msgs::msg::PoseWithConfidence>::SharedPtr pose_sub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr move_info_sub_;
    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agv_mode_sub_;
    rclcpp::Subscription<agv_msgs::msg::InitPose>::SharedPtr floor_sub_;
    rclcpp::Publisher<agv_msgs::msg::PoseWithConfidence>::SharedPtr pose_pub_;
    rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr event_pub_;
    std::vector<rclcpp::TimerBase::SharedPtr> timers_;
    std::map<std::string, FloorInfo> floor_info_;
    KalmanFilter kalmanfilter_;

    Transform tf_odom2map_, tf_base2odom_;
    agv_msgs::msg::OdometryLite current_odometry_;
    agv_msgs::msg::PoseWithConfidence last_pose_msg_, pose_msg_;
    geometry_msgs::msg::Pose current_pose_, target_pose_, origin_pose_;
    std::mutex data_mutex_;
    std::string pose_file_, c1, c2, current_floor_;
    uint32_t current_tag_, origin_tag_, target_tag_, last_tag_, tag_count_, tag_count_num_, qr_count_;
    uint32_t to_node_, from_node_;
    bool use_odom_shift_;
    bool qr_initialized_, parameters_state_;
    bool origin_init_flag_, agv_mode_flag_, has_odometry_, qr_init_flag_;
    int current_mode_, last_mode_;
    int agv_navigation_type_;
    int qr_code_miss_count_, qr_code_miss_threshold_, filter_switch_;
    int qr_code_error_state_;
    
    float init_trans_x_ = 0, init_trans_y_ = 0, init_trans_angle_ = 0;
    double qr_code_miss_distance_, filter_scale_;
    std::set<int> missed_tags_;
    rclcpp::Time last_update_time_;
    double pose_yaw_;
};

#endif